#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include <ctime>
#include <math.h>

#include "leg_ctrl.h"
#include "common.h"

using namespace std;

extern Leg_Ctrl *g_leg_ctrl;

/* 正逆运动测试*/
void test_ik()
{
    Leg_Motor_Desc leg_motor = {
        .motor_n = 3,
        .id_array = {1, 2, 3},      //不重复且大于零
        .type_array = {MOTOR_TYPE_AK, MOTOR_TYPE_YBT, MOTOR_TYPE_YBT},
    };
    Leg_IK_Desc leg_ik = {
        .l_abcd = L_ABCD,
        .l_hip  = L_HIP,
        .l_knee = L_KNEE,
    };
    Leg_Can_Desc leg_can = {
        .data  = nullptr,
    };
    Leg_Trace_Desc leg_trace;

    double *angle = nullptr;
    double *pos = nullptr;
    g_leg_ctrl = new Leg_Ctrl(leg_motor, leg_can, leg_ik, leg_trace);
    g_leg_ctrl->leg_ctrl_init();

    /* now calc */
    angle = g_leg_ctrl->p_ik_obj->get_ik_param()->angle;
    pos = g_leg_ctrl->p_ik_obj->get_ik_param()->pos;
    angle[0] = 90.0f;
    angle[1] = 90.0f;
    angle[2] = -90.0f;
    g_leg_ctrl->p_ik_obj->forward_kinematics();
    g_leg_ctrl->p_ik_obj->inverse_kinematics();
    
    cout << "pos:" << pos[0] << " " << pos[1] << " " << pos[2] << endl;
    cout << "angle:" << angle[0] << " " << angle[1] << " " << angle[2] << endl;
    sleep(1);
    
    g_leg_ctrl->leg_ctrl_deinit();
    sleep(1);

    delete g_leg_ctrl;
    return;  
}

/* 说明：YBT和AK 电机旋转测试
 *  ak电机 id=1; YBT电机 id=3
 *  基于当前零点旋转和增量旋转：
 *      可基于当前零点角度旋转（需先设置零点, 也可基于增量旋转     [USE_CUR_ORIGIN]
 *      YBT最好是增量旋转，因为YBT的零点设置有一定手法, 设置零点经常会失败, 而且设置完后电机进入伺服模式可能会猛地打转.
 *      AK 电机均可
 *  插值法和比较法：                                            [CAHZHI]
 *      YBT电机使用插值法(不可使用比较法, 即直接给一个大角度让它去旋转, 这会导致瞬间毫秒级转过大角度, 造成电路瞬间功率巨大损坏)，
 *      ak电机均可, 比较法的命令间隔为1s(以固定转速一直转, 速度和位置任意一个先到电机就停止转动了)
 */
void test_motor_rotate()
{
#define USE_MOTOR_INDEX 1       //测试第几个电机
#define USE_CUR_ORIGIN  0       //0:增量 1:使用当前角度为0点
#define CAHZHI          1       //0:比较法 1:插值法
    
    Leg_Motor_Desc leg_motor = {
        .motor_n = 2,
        .id_array = {1, 2},      //不重复且大于零
        .type_array = {MOTOR_TYPE_AK, MOTOR_TYPE_YBT},
    };

    Leg_Can_Desc leg_can = {
        .data  = nullptr,
    };

    Leg_IK_Desc leg_ik;
    Leg_Trace_Desc leg_trace;
    
    double target_rotate;       //目的旋转角度
    double cur_pos;
    double value[3];

    value[0] = -230.0f;  //期望旋转角度
    value[1] = 20.0f;   //角速度
    value[2] = 10.0f;   //角加速度

    target_rotate = value[0];
    g_leg_ctrl = new Leg_Ctrl(leg_motor, leg_can, leg_ik, leg_trace);
    g_leg_ctrl->leg_ctrl_init();

    Motor_Type use_motor_type = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_motor_type();
    double accept_err = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_motor_acceptable_err();

    g_leg_ctrl->p_motor_ctrl->motor_func(USE_MOTOR_INDEX, MOTOR_MODE_EXIT, NULL, 0);
    sleep(3);
    g_leg_ctrl->p_motor_ctrl->motor_func(USE_MOTOR_INDEX, MOTOR_MODE_ENTER, NULL, 0);
    sleep(3);

#if USE_CUR_ORIGIN
    //YBT不要在代码中使用设置零点命令，AK可以
    if(MOTOR_TYPE_AK == use_motor_type) {
        g_leg_ctrl->p_motor_ctrl->motor_func(USE_MOTOR_INDEX, MOTOR_MODE_ORIGIN, NULL, 0);
        /* 等待当前pos被代码中设置为0点 */
        do{
            sleep(1);
        }while(g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_cur_pos() > 1.0f);
    } else if(MOTOR_TYPE_YBT == use_motor_type){
        printf("MOTOR_TYPE_YBT cannot use USE_CUR_ORIGIN\n");
    } else {
        ;
    }
#endif

    double start_pos = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_cur_pos();
    double target_pos = target_rotate + start_pos;

#if CAHZHI
    /* 插值法 */
    uint8_t time = 0;
    double freq = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_motor_cmd_freq();
    double each_max = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_motor_each_max();
    uint32_t duration = (((value[0] > 0.0f ? value[0] : -value[0])/value[1]) / freq);   //次数 (value[0]/value[1] + 1);
    if(((value[0] > 0.0f ? value[0] : -value[0])/duration) > each_max) {                    //每次度数
        duration = ((value[0] > 0.0f ? value[0] : -value[0])/each_max + 1);
        freq = each_max/value[1];
    }
    double percent = (double)1.0f/duration;

    std::time_t t0 = std::time(0);
    for(uint32_t i = 1; i <= duration; i++) {
        value[0] = ((1-i*percent)*start_pos) + (i*percent * target_pos);
        g_leg_ctrl->p_motor_ctrl->motor_func(USE_MOTOR_INDEX, MOTOR_MODE_MIX, value, 3);
        usleep(freq*1000*1000);
    }

    sleep(1);
    value[0] = target_pos;
    while(1) {
        cur_pos = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_cur_pos();
        if((cur_pos > (target_pos + each_max)) || (cur_pos < (target_pos - each_max))) { //旋转失败，做保护，防止电机一下转的角度太大 
            break;
        }
        if((cur_pos <  (target_pos + accept_err))  && (cur_pos >  (target_pos - accept_err))) {   //说明旋转ok
            break;
        } else {
            g_leg_ctrl->p_motor_ctrl->motor_func(USE_MOTOR_INDEX, MOTOR_MODE_MIX, value, 3);
            usleep(each_max/value[1]*1000*1000);    
            time++;
            if(time > 3) {      //如果3次还没有旋转到，就退出
                time = 0;
                break;
            }
        }
    }
    std::time_t t1 = std::time(0);
    sleep(1);
    cur_pos = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_cur_pos();
    printf("[motor:%d] target:%f, cur:%f, rotate:%f, time_use:%lds\n", USE_MOTOR_INDEX, target_pos, cur_pos, target_rotate, t1 - t0);
#else
    /* 仅AK电机可使用比较法 以固定转速一直转，每次命令间隔1s*/
    value[0] = target_pos;
    if(MOTOR_TYPE_AK == use_motor_type) {
        std::time_t t0 = std::time(0); 
        while(1) {
            cur_pos = g_leg_ctrl->p_motor_ctrl->get_motor_item(USE_MOTOR_INDEX)->get_cur_pos();
            if((cur_pos <  (target_pos + accept_err))  && (cur_pos >  (target_pos - accept_err))) {
                break;
            } else {
                g_leg_ctrl->p_motor_ctrl->motor_func(0, MOTOR_MODE_MIX, value, 3);
                usleep(1*1000*1000);
            }
        }
        std::time_t t1 = std::time(0); 
        printf("[motor:%d] target:%f, cur:%f, rotate:%f, time_use:%lds\n", USE_MOTOR_INDEX, target_pos, cur_pos, target_rotate, t1 - t0);
    } else if(MOTOR_TYPE_YBT == use_motor_type){
        printf("MOTOR_TYPE_YBT can only use CAHZHI\n");
    } else {
        ;
    }
#endif
    sleep(1);
    g_leg_ctrl->leg_ctrl_deinit();
    delete g_leg_ctrl;

    return ;    
}

/* 轨迹生成测试 */
void test_trace()
{
    Leg_Motor_Desc leg_motor = {
        .motor_n = 3,
        .id_array = {1, 2, 3},      //不重复且大于零
        .type_array = {MOTOR_TYPE_AK, MOTOR_TYPE_YBT, MOTOR_TYPE_YBT},
    };

    Leg_Can_Desc leg_can = {
        .data  = nullptr,
    };

    Leg_IK_Desc leg_ik = {
        .l_abcd = L_ABCD,
        .l_hip  = L_HIP,
        .l_knee = L_KNEE,
    };

    Leg_Trace_Desc leg_trace = {
        .len = 5,
        .param = {(sqrt(pow(leg_ik.l_hip, 2) + pow(leg_ik.l_knee, 2)) / (2*PAI)), 60.0f, -12.55f, 6.35f, -21.65f},            //{r,w,x y,z}
        .type = TRACE_CYCLOID, 
    };    

    double *angle = nullptr;
    double *pos = nullptr;
    g_leg_ctrl = new Leg_Ctrl(leg_motor, leg_can, leg_ik, leg_trace);
    g_leg_ctrl->leg_ctrl_init();

    /* now calc */
    angle = g_leg_ctrl->p_ik_obj->get_ik_param()->angle;
    pos = g_leg_ctrl->p_ik_obj->get_ik_param()->pos;

    //摆线初始状态
    angle[0] = 0.0f;
    angle[1] = 67.0f;
    angle[2] = -90.0f;
    g_leg_ctrl->p_ik_obj->forward_kinematics();
    g_leg_ctrl->p_ik_obj->inverse_kinematics();
    
    cout << "pos:" << pos[0] << " " << pos[1] << " " << pos[2] << endl;
    cout << "angle:" << angle[0] << " " << angle[1] << " " << angle[2] << endl;

    double freq = 0.1;
    double *trace_pos;
    uint32_t cnt = 0;
    double time = 0;
    sleep(1);
    std::time_t t0 = std::time(0);
    while(1) {
        cnt = g_leg_ctrl->p_trace_obj->trace_generate(time);
        trace_pos = g_leg_ctrl->p_trace_obj->get_pos();
        pos[0] = trace_pos[0]; pos[1] = trace_pos[1]; pos[2] = trace_pos[2];
        g_leg_ctrl->p_ik_obj->inverse_kinematics();
        printf("pos:%f, %f, %f\t", trace_pos[0], trace_pos[1], trace_pos[2]);
        printf("ang:%f, %f, %f\n", angle[0], angle[1], angle[2]);
        usleep(freq*1000*1000);
        time += freq;
        
        if(cnt > 1) {
            break;
        }
    }
    std::time_t t1 = std::time(0); 
    printf("trace time_use:%lds\n", t1 - t0);

    g_leg_ctrl->leg_ctrl_deinit();

    delete g_leg_ctrl;
    
    return ;
}

/* 测试腿的运动 */
void test_leg_trace()
{
    Leg_Motor_Desc leg_motor = {
        .motor_n = 3,
        .id_array = {1,2,3},      //不重复且大于零
        .type_array = {MOTOR_TYPE_YBT, MOTOR_TYPE_YBT, MOTOR_TYPE_YBT},
    };

    Leg_Can_Desc leg_can = {
        .data  = nullptr,
    };

    Leg_IK_Desc leg_ik = {
        .l_abcd = L_ABCD,
        .l_hip  = L_HIP,
        .l_knee = L_KNEE,
    };

    Leg_Trace_Desc leg_trace = {
        .len = 5,
        .param = {0},            //{r,w,x y,z}
        .type = TRACE_CYCLOID, 
    };    

    g_leg_ctrl = new Leg_Ctrl(leg_motor, leg_can, leg_ik, leg_trace);
    g_leg_ctrl->leg_ctrl_init(); 

    //...wait until thread exit;
    g_leg_ctrl->leg_thread_enable();
    
    g_leg_ctrl->leg_ctrl_deinit();
    
    delete g_leg_ctrl;
    return ;  
}